//*****************************************************************
// PIDControlAlgorithm.cpp
//
//  Created on: Jan 21, 2012
//      Author: Christopher Wood
//              Robert Livosi
//
//  $Id: PIDControlAlgorithm.cpp 32 2012-01-30 04:49:23Z caw4567@gmail.com $
//*****************************************************************

#include "PIDControlAlgorithm.h"

/**
 * Default constructor.
 */
PIDControlAlgorithm::PIDControlAlgorithm()
{
	// Initialize all transform variables and buffers.
	uIndex = 0;
	eIndex = 0;
	memset(uBuffer, 0, sizeof(float) * OUTPUT_SIZE);
	memset(eBuffer, 0, sizeof(float) * OUTPUT_SIZE);

	// Initialize the PID parameters to some known good values.
	this->kp = 1.0;
	this->ki = 0.0;
	this->kd = 0.0;

	// Init the update semaphore
	sem_init(&updateSem, 0, 1);
}

/**
 * Default destructor.
 */
PIDControlAlgorithm::~PIDControlAlgorithm()
{
}

/**
 * Method that is used to prompt the user for the
 * new algorithm parameters.
 *
 * @return a string for the user to interpret.
 */
string PIDControlAlgorithm::getParamString()
{
	return "Enter a set of parameters (Kp, Ki, Kd): ";
}

/**
 * Helper method to determine the number of parameters
 *
 * @return the number of parameters needed to collect
 */
int PIDControlAlgorithm::getParamLength()
{
	return NUM_PARAMS;
}

/**
 * Set the parameters of the algorithm.
 *
 * @param params - string of new parameters
 * @returns true if parsing was successful, false otherwise.
 */
bool PIDControlAlgorithm::setParams(vector<string> params)
{
	bool successful = true;
	float newKp = 0.0;
	float newKi = 0.0;
	float newKd = 0.0;

	// Try to parse the new parameters
	try
	{
		int index = 0;
		for (vector<string>::iterator itr = params.begin(); itr != params.end(); itr++)
		{
			switch (index)
			{
			case 0:
				newKp = atof((*itr).c_str());
				break;
			case 1:
				newKi = atof((*itr).c_str());
				break;
			case 2:
				newKd = atof((*itr).c_str());
				break;
			}
			index++;
		}
	}
	catch (...)
	{
		successful = false;
	}

	// Only update if the parsing was successful
	if (successful)
	{
		sem_wait(&updateSem);
		this->kp = newKp;
		this->ki = newKi;
		this->kd = newKd;
		memset(uBuffer, 0, sizeof(float) * OUTPUT_SIZE);
		memset(eBuffer, 0, sizeof(float) * OUTPUT_SIZE);
		sem_post(&updateSem);
	}

	return successful;
}

/**
 * Compute the transform based on the specified input.
 *
 * @param reference - the desired set point
 * @param input - the new input
 * @return the new output
 */
float PIDControlAlgorithm::computeTransform(float reference, float input)
{
	float error = 0.0;
	float result = 0.0;

	// Calculate error
	error = reference - input;

	// Perform the conversion
	sem_wait(&updateSem);
	uBuffer[uIndex] = ((((kp + ki + kd) * error) -
			(kp + (2 * kd)) * eBuffer[(eIndex + OUTPUT_SIZE - 1) % OUTPUT_SIZE] +
			(kd * eBuffer[(eIndex + OUTPUT_SIZE - 2) % OUTPUT_SIZE]))) +
			uBuffer[(uIndex + OUTPUT_SIZE - 1) % OUTPUT_SIZE];
	sem_post(&updateSem);

	// Check for and handle clipping
	if (uBuffer[uIndex] > UPPER_VOLTAGE_CLIP)
	{
		uBuffer[uIndex] = UPPER_VOLTAGE_CLIP;
	}
	else if (uBuffer[uIndex] < LOWER_VOLTAGE_CLIP)
	{
		uBuffer[uIndex] = LOWER_VOLTAGE_CLIP;
	}
	result = uBuffer[uIndex];

	// Update history indices
	eBuffer[eIndex] = error;
	uIndex = (uIndex + 1) % OUTPUT_SIZE;
	eIndex = (eIndex + 1) % OUTPUT_SIZE;

	return result;
}

/**
 * Reset the control algorithm history buffers.
 */
void PIDControlAlgorithm::reset()
{
	this->kp = 0.0;
	this->ki = 0.0;
	this->kd = 0.0;
	this->uIndex = 0;
	this->eIndex = 0;
	memset(uBuffer, 0, sizeof(float) * OUTPUT_SIZE);
	memset(eBuffer, 0, sizeof(float) * OUTPUT_SIZE);
}

